

#include <sensor_msgs/Image.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Range.h>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Accel.h>
#include <signal.h>

#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Float64MultiArray.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include "ros/ros.h"
#include "std_msgs/Int8.h"
#include <boost/format.hpp>
#include <queue>
#include <webots_ros/Int32Stamped.h>

#include <webots_ros/set_float.h>

#include <webots_ros/set_int.h>
#include <webots_ros/set_bool.h>
#include <webots_ros/get_float.h>
#include <webots_ros/robot_get_device_list.h>
#include <iostream>
using namespace std;
#define TIME_STEP 32

#define NMOTORS 6

#define MAX_SPEED 10


ros::NodeHandle *n;

static std::vector<float> lidarValues;
static int lms291Resolution = 0;
static int halfResolution = 0;
static double maxRange = 0.0;
static double rangeThreshold = 0.0;
static double GPSValues[3] = {0, 0, 0};
static std::vector<std::string> controllerList;
static double inertialUnitValues[4] = {0, 0, 0, 0};
std::queue<std::vector<float> >Acc_q;
std::queue<std::vector<float> >Ang_q;
static std::vector<double> braitenbergCoefficients;
static bool areBraitenbergCoefficientsinitialized = false;


static int controllerCount;
static std::string controllerName;
sensor_msgs::PointCloud2  output;
double gaussian(double x, double mu, double sigma) {
  return (1.0 / (sigma * sqrt(2.0 * M_PI))) * exp(-((x - mu) * (x - mu)) / (2 * sigma * sigma));
}

ros::ServiceClient timeStepClient;

webots_ros::set_int timeStepSrv;

int vleft,vright;
float t1,t2,t3,t4,t5,t6;
float v1_back,v2_back,v3_back,v4_back,v5_back,v6_back,vl_back,vr_back;
//double speed_x,speed_y,speed_w;

float WHEEL_L=2.8;
float WHEEL_R=0.6;
float WHEEL_PI=3.141592693;


ros::Publisher lidar3d_pub;
ros::Publisher Imu_pub;
static const char *motorNames[NMOTORS] ={"wheel1", "wheel2", "wheel3","wheel4","wheel5","wheel6"};//匹配之前定义好的电机name

float motor_back[NMOTORS]={0,0,0,0,0,0};

void updateSpeed() {
double speeds[NMOTORS];

speeds[0] = t1;//vleft
speeds[1] = t2;//vright
speeds[2] = t3;
speeds[3] = t4;
speeds[4] = t5;
speeds[5] = t6;
 
 //set speeds

for (int i = 0; i < NMOTORS; ++i) {
	ros::ServiceClient set_velocity_client;
	webots_ros::set_float set_velocity_srv;
	set_velocity_client = n->serviceClient<webots_ros::set_float>(std::string("ros_test/")
	+ std::string(motorNames[i]) + std::string("/set_velocity"));
	set_velocity_srv.request.value = speeds[i];
	set_velocity_client.call(set_velocity_srv);
 }

}//将速度请求以set_float的形式发送给set_velocity_srv或set_torque_client




// catch names of the controllers availables on ROS network
void controllerNameCallback(const std_msgs::String::ConstPtr &name) {
controllerCount++;
controllerList.push_back(name->data);
ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());

}

//void MotionPubLeftCallback(const std_msgs::Int8::ConstPtr& msg) {
 //ROS_INFO("I heard left V: [%d]", msg->data);
//vright=msg->data;
//}

//void MotionPubRightCallback(const std_msgs::Int8::ConstPtr& msg) {
 //ROS_INFO("I heard right V: [%d]", msg->data);
//vleft=msg->data;
//}

void wheel_torque_publisherCallback(const std_msgs::Float64MultiArray::ConstPtr& msg)
{
	//你的回调函数
	t1=msg->data[0];
	t2=msg->data[1];
	t3=msg->data[2];
	t4=msg->data[3];
	t5=msg->data[4];
	t6=msg->data[5];

	updateSpeed();
}

void quit(int sig) {

ROS_INFO("User stopped the 'ros_test' node.");

timeStepSrv.request.value = 0;
timeStepClient.call(timeStepSrv);
ros::shutdown();
exit(0);
}


void broadcastTransform() {
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin(tf::Vector3(-GPSValues[2], GPSValues[0], GPSValues[1]));
  tf::Quaternion q(inertialUnitValues[0], inertialUnitValues[1], inertialUnitValues[2], inertialUnitValues[3]);
  q = q.inverse();
  transform.setRotation(q);
  //br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "base_link"));
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "odom"));
  transform.setIdentity();
  //br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "/ros_key_test/lidar3d"));
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "/ros_test/Sick_LMS_291"));

  tf::Quaternion q1(-0.5, 0.5, 0.5, 0.5);
  q1 = q1.inverse();
  transform.setRotation(q1);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "imu_link"));
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link",  "/ros_test/IMU"));
  //3d雷达与baselink的tf坐标转换
  tf::Quaternion q2(-0.5, 0.5, 0.5, 0.5);
  q2 = q2.inverse();
  transform.setRotation(q2);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "ros_test/lidar3d"));
}


void GPSCallback(const sensor_msgs::NavSatFix::ConstPtr &values) {
  GPSValues[0] = values->latitude;
  GPSValues[1] = values->altitude;
  GPSValues[2] = values->longitude;
  broadcastTransform();
}


void AngCallback(const sensor_msgs::Imu::ConstPtr &values) {
  //ROS_INFO("Ang_lineCallback.");
  std::vector<float> AngValues;
  AngValues.resize(3);

  AngValues[0]= values->angular_velocity.x;
  AngValues[1]= values->angular_velocity.y;
  AngValues[2]= values->angular_velocity.z;
  Ang_q.push(AngValues);
}


void lidarCallback(const sensor_msgs::LaserScan::ConstPtr &scan) {
  int scanSize = scan->ranges.size();
  lidarValues.resize(scanSize);
  for (int i = 0; i < scanSize; ++i)
    lidarValues[i] = scan->ranges[i];

  lms291Resolution = scanSize;
  halfResolution = scanSize / 2;
  maxRange = scan->range_max;
  rangeThreshold = maxRange / 20.0;
  if (!areBraitenbergCoefficientsinitialized) {
    braitenbergCoefficients.resize(lms291Resolution);
    for (int i = 0; i < lms291Resolution; ++i)
      braitenbergCoefficients[i] = gaussian(i, halfResolution, lms291Resolution / 5);
    areBraitenbergCoefficientsinitialized = true;
  }
}


void lidar3dCallback(const sensor_msgs::PointCloud &scan) {
   // ROS_INFO("GET POINT CLOUD DATA.");
    sensor_msgs::PointCloud2 output;
    //.... do something with the input and generate the output...
    convertPointCloudToPointCloud2(scan,output);
    lidar3d_pub.publish (output);
}

void Acc_lineCallback(const sensor_msgs::Imu::ConstPtr &values) {
 // ROS_INFO("Acc_lineCallback.");
  std::vector<float> AccValues;
  AccValues.resize(3);
  AccValues[0]=values->linear_acceleration.x;
  AccValues[1]=values->linear_acceleration.y;
  AccValues[2]=values->linear_acceleration.z;
  Acc_q.push(AccValues);
}

void inertialUnitCallback(const sensor_msgs::Imu::ConstPtr &values) {

  inertialUnitValues[0] = values->orientation.x;
  inertialUnitValues[1] = values->orientation.y;
  inertialUnitValues[2] = values->orientation.z;
  inertialUnitValues[3] = values->orientation.w;

  sensor_msgs::Imu ImuDate;
  ImuDate.orientation= values->orientation;
  ImuDate.orientation_covariance= values->orientation_covariance;

  ImuDate.linear_acceleration_covariance= values->linear_acceleration_covariance;
  ImuDate.linear_acceleration_covariance[0]=0.0;

  if(!Acc_q.empty())
  {
    ImuDate.linear_acceleration.x= Acc_q.front()[0];
    ImuDate.linear_acceleration.y= Acc_q.front()[1];
    ImuDate.linear_acceleration.z= Acc_q.front()[2];
    std::cout<<"imu_acc:"<<Acc_q.front()[0]<<" "<<Acc_q.front()[1]<<" "<<Acc_q.front()[2]<<std::endl;
    Acc_q.pop();
  }

  if(!Ang_q.empty())
  {
    ImuDate.angular_velocity.x= Ang_q.front()[0];
    ImuDate.angular_velocity.y= Ang_q.front()[1];
    ImuDate.angular_velocity.z= Ang_q.front()[2];
    Ang_q.pop();
  }
  else
  {
      std::cout<<"Ang_q is empty!"<<std::endl;

  }
  ImuDate.angular_velocity_covariance= values->angular_velocity_covariance;
  ImuDate.angular_velocity_covariance[0]=0.0;
  Imu_pub.publish(ImuDate);
  broadcastTransform();
}


int main(int argc, char **argv) {

std::string controllerName;
float speed_response[6];

// create a node named 'ros_test' on ROS network


ros::init(argc, argv, "ros_test",
ros::init_options::AnonymousName);

n = new ros::NodeHandle;
signal(SIGINT, quit);

// subscribe to the topic model_name to get the list of availables controllers

ros::Subscriber nameSub = n->subscribe("model_name", 100, controllerNameCallback);

//ros::Subscriber vleftSub = n->subscribe("/MotionPubLeft", 100, MotionPubLeftCallback);

//ros::Subscriber vrightSub = n->subscribe("/MotionPubRight", 100, MotionPubRightCallback);

ros::Subscriber sub = n->subscribe("wheel_torque_publisher", 1000, wheel_torque_publisherCallback);

while (controllerCount == 0 || controllerCount <nameSub.getNumPublishers()) {
ros::spinOnce();
ros::spinOnce();
ros::spinOnce();
 }
ros::spinOnce();

timeStepClient = n->serviceClient<webots_ros::set_int>("ros_test/robot/time_step");
timeStepSrv.request.value = TIME_STEP;


// if there is more than one controller available, it let the user choose
if (controllerCount == 1)
controllerName = controllerList[0];

else {

int wantedController = 0;

std::cout << "Choose the # of the controller you want touse:\n";
std::cin >> wantedController;
if (1 <= wantedController && wantedController <= controllerCount)
controllerName = controllerList[wantedController - 1];
else {

ROS_ERROR("Invalid number for controller choice.");

return 1;

}
}
ROS_INFO("Using controller: '%s'", controllerName.c_str());

// leave topic once it is not necessary anymore

nameSub.shutdown();

// init motors
for (int i = 0; i < NMOTORS; ++i) {

// position，发送电机位置给wheel1-6，速度控制时设置为缺省值INFINITY
ros::ServiceClient set_position_client;
webots_ros::set_float set_position_srv;
set_position_client = n->serviceClient<webots_ros::set_float>(std::string("ros_test/")
+ std::string(motorNames[i]) + std::string("/set_position"));
set_position_srv.request.value = INFINITY;
if (set_position_client.call(set_position_srv) &&set_position_srv.response.success)
		ROS_INFO("Position set to INFINITY for motor %s.",motorNames[i]);
else
		ROS_ERROR("Failed to call service set_position on motor %s.",motorNames[i]);


// speed，发送电机初始速度０给wheel1-6
ros::ServiceClient set_velocity_client;
webots_ros::set_float set_velocity_srv;
set_velocity_client =
n->serviceClient<webots_ros::set_float>(std::string("ros_test/")
+ std::string(motorNames[i]) + std::string("/set_velocity"));
set_velocity_srv.request.value = 0.0;
if (set_velocity_client.call(set_velocity_srv) &&set_velocity_srv.response.success == 1)
	ROS_INFO("Velocity set to 0.0 for motor %s.", motorNames[i]);
else
	ROS_ERROR("Failed to call service set_velocity on motor %s.",motorNames[i]);


// torque，发送电机初始转矩０给wheel1-6
//ros::ServiceClient set_torque_client;
//webots_ros::set_float set_torque_srv;
//set_torque_client =
//n->serviceClient<webots_ros::set_float>(std::string("ros_test/")
//+ std::string(motorNames[i]) + std::string("/set_torque"));
//set_torque_srv.request.value = 0.0;
//if (set_torque_client.call(set_torque_srv) &&
//set_torque_srv.response.success == 1)
//ROS_INFO("torque set to 0.0 for motor %s.", motorNames[i]);
//else
//ROS_ERROR("Failed to call service set_torque on motor %s.",
//motorNames[i]);
}

// enable accelerometer
  ros::ServiceClient set_accelerometer_client;
  webots_ros::set_int accelerometer_srv;
  ros::Subscriber sub_accelerometer;
  set_accelerometer_client = n->serviceClient<webots_ros::set_int>("ros_test/accelerometer/enable");
  accelerometer_srv.request.value = 32;
  if (set_accelerometer_client.call(accelerometer_srv) && accelerometer_srv.response.success) {
      sub_accelerometer = n->subscribe("ros_test/accelerometer/values", 5, Acc_lineCallback);
      while (sub_accelerometer.getNumPublishers() == 0) {
      }
      ROS_INFO("Acc enabled.");
    } else {
      if (!accelerometer_srv.response.success)
        ROS_ERROR("Sampling period is not valid.");
      ROS_ERROR("Failed to enable Acc.");
      return 1;
    }

  // enable gyro
  ros::ServiceClient set_gyro_client;
  webots_ros::set_int gyro_srv;
  ros::Subscriber sub_gyro;
  set_gyro_client = n->serviceClient<webots_ros::set_int>("ros_test/gyro/enable");
  gyro_srv.request.value = 32;
  if (set_gyro_client.call(gyro_srv) && gyro_srv.response.success) {
    ROS_INFO("Gyro subscribe.");
    sub_gyro = n->subscribe("ros_test/gyro/values", 5, AngCallback);
    while (sub_gyro.getNumPublishers() == 0) {

    }
    ROS_INFO("Gyro enabled.");
  } else {
    if (!gyro_srv.response.success)
      ROS_ERROR("Sampling period is not valid.");
    ROS_ERROR("Failed to enable Gyro.");
    return 1;
  }
  // enable inertial unit
  ros::ServiceClient set_inertial_unit_client;
  webots_ros::set_int inertial_unit_srv;
  ros::Subscriber sub_inertial_unit;
  set_inertial_unit_client = n->serviceClient<webots_ros::set_int>("ros_test/inertial_unit/enable");
  inertial_unit_srv.request.value = 32;
  if (set_inertial_unit_client.call(inertial_unit_srv) && inertial_unit_srv.response.success) {
    sub_inertial_unit = n->subscribe("ros_test/inertial_unit/roll_pitch_yaw", 1, inertialUnitCallback);
    Imu_pub = n->advertise<sensor_msgs::Imu>("/ros_test/IMU", 10);
    while (sub_inertial_unit.getNumPublishers() == 0) {
    }
    ROS_INFO("Inertial unit enabled.");
  } else {
    if (!inertial_unit_srv.response.success)
      ROS_ERROR("Sampling period is not valid.");
    ROS_ERROR("Failed to enable inertial unit.");
    return 1;
  }


// enable gps
  ros::ServiceClient set_GPS_client;
  webots_ros::set_int GPS_srv;
  ros::Subscriber sub_GPS;
  set_GPS_client = n->serviceClient<webots_ros::set_int>("ros_test/gps/enable");
  GPS_srv.request.value = 32;
  if (set_GPS_client.call(GPS_srv) && GPS_srv.response.success) {
    sub_GPS = n->subscribe("ros_test/gps/values", 1, GPSCallback);
    while (sub_GPS.getNumPublishers() == 0) {
    }
    ROS_INFO("GPS enabled.");
  } else {
    if (!GPS_srv.response.success)
      ROS_ERROR("Sampling period is not valid.");
    ROS_ERROR("Failed to enable GPS.");
    return 1;
  }

  // enable lms
  ros::ServiceClient set_lidar_client;
  webots_ros::set_int lidar_srv;
  ros::Subscriber sub_lidar_scan;

  set_lidar_client = n->serviceClient<webots_ros::set_int>("ros_test/Sick_LMS_291/enable");
  lidar_srv.request.value = TIME_STEP;
  if (set_lidar_client.call(lidar_srv) && lidar_srv.response.success) {
    ROS_INFO("Lidar enabled.");
    sub_lidar_scan = n->subscribe("ros_test/Sick_LMS_291/laser_scan/layer0", 10, lidarCallback);
    ROS_INFO("Topic for lidar initialized.");
    while (sub_lidar_scan.getNumPublishers() == 0) {
      //ROS_INFO("sub_lidar_scan getNumPublishers is zero.");
    }
    ROS_INFO("Topic for lidar scan connected.");
  } else {
    if (!lidar_srv.response.success)
      ROS_ERROR("Sampling period is not valid.");
    ROS_ERROR("Failed to enable lidar.");
    return 1;
  }

  // enable lidar3d
  ros::ServiceClient set_lidar3d_client;
  ros::ServiceClient set_lidar3d1_client;
  webots_ros::set_int lidar3d_srv;
  webots_ros::set_bool lidar3d1_srv;
  ros::Subscriber sub_lidar3d_scan;

  set_lidar3d_client = n->serviceClient<webots_ros::set_int>("ros_test/lidar3d/enable");
  set_lidar3d1_client = n->serviceClient<webots_ros::set_bool>("ros_test/lidar3d/enable_point_cloud");

  lidar3d_srv.request.value = TIME_STEP;
  lidar3d1_srv.request.value = true;
  if (set_lidar3d_client.call(lidar3d_srv) && lidar3d_srv.response.success) {
    ROS_INFO("lidar3d enabled.");
    //SubscribeAndPublish SAPObject(n);
    sub_lidar3d_scan = n->subscribe("ros_test/lidar3d/point_cloud", 10, lidar3dCallback);
    lidar3d_pub = n->advertise<sensor_msgs::PointCloud2>("/ros_test/lidar3d/point_cloud2", 10);

    ROS_INFO("Topic for lidar3d initialized.");
    //while (sub_lidar3d_scan.getNumPublishers() == 0) {
    //}
    ROS_INFO("Topic for lidar3d scan connected.");
      if (set_lidar3d1_client.call(lidar3d1_srv) && lidar3d1_srv.response.success) {
    ROS_INFO("lidar3d pointclouds enabled .");
     }
  } else {
    if (!lidar3d_srv.response.success)
      ROS_ERROR("Sampling period is not valid.");
    ROS_ERROR("Failed to enable lidar3d.");
    return 1;
  }

  // enable camera
  ros::ServiceClient set_camera_client;
  webots_ros::set_int camera_srv;
  ros::Subscriber sub_camera;
  set_camera_client = n->serviceClient<webots_ros::set_int>("ros_test/camera/enable");
  camera_srv.request.value = 64;
  set_camera_client.call(camera_srv);



//prepare to updata odom
ros::Publisher odom_pub = n->advertise<nav_msgs::Odometry>("odom", 50);
tf::TransformBroadcaster odom_broadcaster;

double x = 0.0;
double y = 0.0;
double th = 0.0;

ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();

ros::Rate loop_rate(100);

// main loop
while (ros::ok())
{
	if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success)
	{
		ROS_ERROR("Failed to call service time_step for next step.");
		break;
	}
	//get odom data
	for (int i = 0; i < NMOTORS; ++i) {
		ros::ServiceClient get_velocity_client;
		webots_ros::get_float get_velocity_srv;
		get_velocity_client =
		n->serviceClient<webots_ros::get_float>(std::string("ros_test/")
		+ std::string(motorNames[i]) + std::string("/get_velocity"));

		if (get_velocity_client.call(get_velocity_srv)){
			motor_back[i]=get_velocity_srv.response.value;
		}
		else
		{
			ROS_ERROR("Failed to call service get_velocity on motor %s.",motorNames[i]);
		}
	}

	v1_back=motor_back[0]*WHEEL_R;
	v2_back=motor_back[1]*WHEEL_R;
	v3_back=motor_back[2]*WHEEL_R;
	v4_back=motor_back[3]*WHEEL_R;
	v5_back=motor_back[4]*WHEEL_R;
	v6_back=motor_back[5]*WHEEL_R;

	//vl_back=(v1_back+v3_back+v5_back)/3.0;
	//vr_back=(v2_back+v4_back+v6_back)/3.0;
	vl_back=v3_back;
	vr_back=v4_back;
  	ROS_INFO("v1:  %f\t v2:  %f\tv3:  %f\tv4:  %f\tv5:  %f\tv6:  %f\t ",v1_back,v2_back,v3_back,v4_back,v5_back,v6_back);

	double speed_x = (vl_back+vr_back)*0.5*0.5;
	double speed_y = 0;
	double speed_w = (vr_back-vl_back)/WHEEL_L/4;
	ROS_INFO("speed_x %f\t speed_w %f",speed_x*2,speed_w*4);

	//updata odom
	current_time = ros::Time::now();
	double dt = (current_time - last_time).toSec();
	//double dt =0.1;
	double delta_x = (speed_x * cos(th) - speed_y* sin(th)) * dt; //*0.6
	double delta_y = (speed_x * sin(th) + speed_y * cos(th)) * dt; //*0.6
	double delta_th = speed_w * dt;    //*0.6
   	ROS_INFO("dt %f",dt);
	
	x += delta_x*0.6;
	y += delta_y*0.6;
	th += delta_th*0.6;
	
	 if(th>2*WHEEL_PI)
			th=th-2*WHEEL_PI;	
	else if(th<-2*WHEEL_PI)
			th=th+2*WHEEL_PI;
	else;
  
   ROS_INFO("x: %f \t y: %f \tw: %f",x,y,th);
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
    geometry_msgs::TransformStamped odom_trans;

    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";
    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat;
    odom_broadcaster.sendTransform(odom_trans);

    nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

    odom.pose.pose.position.x = x;
    odom.pose.pose.position.y = y;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;
    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = (speed_x * cos(th) - speed_y* sin(th))*2;
    odom.twist.twist.linear.y = (speed_x * sin(th) + speed_y * cos(th))*2;
    odom.twist.twist.angular.z = speed_w*4;

    //publish the message
    odom_pub.publish(odom);
    last_time = current_time;
  
	ros::spinOnce();
}
timeStepSrv.request.value = 0;
timeStepClient.call(timeStepSrv);
ros::shutdown();
return 0;

}





